#include "rapter/io/inputParser.hpp"
#include "rapter/util/parse.h"
#include "rapter/typedefs.h"
//#include "rapter/my_types.h"
#include "rapter/processing/graph.hpp"
#include "rapter/util/containers.hpp" // class PrimitiveContainer
#include "rapter/util/pclUtil.h"

namespace rapter
{

template <typename _Scalar>
struct RepresentParams
{
    _Scalar scale;
    AnglesT angles;
};


 template <
            class _PrimitiveContainerT
          , class _PointContainerT
          , class _PrimitiveT /*         = typename _PrimitiveContainerT::value_type::value_type*/
          , class _PointPrimitiveT     /*= typename _PointContainerT::value_type*/
          , class _FiniteFiniteDistFunctor
          >
 static inline int myRepresentCli(
         std::string                     cloud_path,
         std::string                     input_prims_path,
         std::string                     associations_path,
         float                           scalar,
         AnglesT                         angle_gens
         )
 {
     // input
     typedef typename _PrimitiveContainerT::value_type        InnerPrimitiveContainerT;
     typedef containers::PrimitiveContainer<_PrimitiveT>      PrimitiveMapT;
     typedef typename _PrimitiveT::Scalar                     Scalar;
     typedef typename _PrimitiveT::ExtremaT                   ExtremaT;
     PointContainerT         points;
     PclCloudPtrT            pcl_cloud;
     _PrimitiveContainerT    prims;
     PrimitiveMapT           patches;
     RepresentParams<Scalar> params;

     // Graphs
     typedef Graph<Scalar, typename MyGraphConfig<Scalar>::UndirectedGraph > GraphT;
     typedef typename graph::EdgeT<Scalar>                                   EdgeT;
     typedef typename GraphT::ComponentListT                                 ComponentListT;
     typedef typename GraphT::ClustersT                                      ClustersT;

     //int ret = rapter::parseInput<InnerPrimitiveContainerT,PclCloudT>(
     //            points, pcl_cloud, prims, patches, params, argc, argv );

     bool valid_input = true;
     if (!boost::filesystem::exists(cloud_path) )
     {
         std::cerr << "[" << __func__ << "]: " << "--cloud does not exist: " << cloud_path << std::endl;
         valid_input = false;
     }
     if (!input_prims_path.size() || (!boost::filesystem::exists(input_prims_path)) )
     {
         std::cerr << "[" << __func__ << "]: " << "-p or --prims is compulsory" << std::endl;
         valid_input = false;
     }
     if (!boost::filesystem::exists(associations_path))
     {
         std::cerr << "[" << __func__ << "]: " << "-a or --assoc is compulsory" << std::endl;
         valid_input = false;
     }
     params.scale = scalar;

     int ret = rapter::myParseInput<InnerPrimitiveContainerT,PclCloudT>(
                 points, pcl_cloud, prims, patches, cloud_path,input_prims_path,associations_path);



     std::cout << "[" << __func__ << "]: " << "parseInput ret: " << ret << std::endl;
     valid_input = (EXIT_SUCCESS == ret);

     params.angles = angle_gens;

     if ( !valid_input )
     { std::cout << "Usage: --represent[3D] -p prims.csv -a points_primitives.csv -sc scale --cloud cloud.ply --angle-gens 90" << std::endl; return EXIT_FAILURE; }

     GidPidVectorMap populations;
     processing::getPopulations( populations, points );

     std::cout << "[" << __func__ << "]: " << "gids: " << patches.size() << ", points: " << points.size() << ", scale: " << params.scale << std::endl;


     /// WORK
     // output representatives
     _PrimitiveContainerT outPrims;
     std::set<GidT> activeGids;

     typedef typename Eigen::Matrix<Scalar,Eigen::Dynamic,1> SpatialSignifT;
     SpatialSignifT spatialSignif(1,1); // tmp
     typedef typename std::pair<SpatialSignifT,_PrimitiveT const*> SizedPrimT;
     std::map< DidT, SizedPrimT > maxSpatialSignifs; // "size"

     for ( typename PrimitiveMapT::Iterator it0(patches); it0.hasNext(); it0.step() )
     {
         if ( it0->getTag(_PrimitiveT::TAGS::STATUS) == _PrimitiveT::STATUS_VALUES::SMALL ) continue; // added 9 / 1 / 2015

         // cache
         _PrimitiveT const* prim = &(*it0);
         const DidT did = prim->getTag( _PrimitiveT::TAGS::DIR_GID );
         // calc size
         prim->getSpatialSignificance( spatialSignif, points, params.scale, &(populations[prim->getTag(_PrimitiveT::TAGS::GID)]) );

         // insert, if did unseen
         if ( maxSpatialSignifs.find( did ) == maxSpatialSignifs.end() )
             maxSpatialSignifs[ did ] = SizedPrimT( spatialSignif, prim );
         // or replace max, if larger
         else if ( spatialSignif(0) > maxSpatialSignifs[did].first(0) )
         {
             maxSpatialSignifs[did].first  = spatialSignif; // store size
             maxSpatialSignifs[did].second = prim;          // store primitive
         } //...if bigger
     } //...all primitives

     // record to output
     for ( auto it = maxSpatialSignifs.begin(); it != maxSpatialSignifs.end(); ++it )
     {
         //  first: did
         // second: SizedPrimT (<size,prim>)
         _PrimitiveT const* prim = it->second.second;
         containers::add( outPrims, prim->getTag(_PrimitiveT::TAGS::GID), *(prim) );
         activeGids.insert( prim->getTag(_PrimitiveT::TAGS::GID) );
     }

     std::cout << "[" << __func__ << "]: " << "saved to representatives.csv" << std::endl;
     std::string prefix=cloud_path.substr(0,cloud_path.find_last_of("/")+1);
     io::savePrimitives<_PrimitiveT,typename InnerPrimitiveContainerT::const_iterator>( outPrims, prefix + "representatives.csv" );

     // ___POINTS___
     _PointContainerT outPoints( points ); // need reassignment
     // clear all points' assignment that don't have selected primitives
     for ( auto it = outPoints.begin(); it != outPoints.end(); ++it )
     {
         if ( activeGids.find( it->getTag(_PointPrimitiveT::TAGS::GID) ) == activeGids.end() )
             it->setTag( _PointPrimitiveT::TAGS::GID, _PointPrimitiveT::LONG_VALUES::UNSET );
     }
     std::string assocPath( "points_representatives.csv" );
     io::writeAssociations<_PointPrimitiveT>( outPoints, prefix + assocPath );

     return !valid_input;
 } //...representCli

 template <
            class _PrimitiveContainerT
          , class _PointContainerT
          , class _PrimitiveT /*         = typename _PrimitiveContainerT::value_type::value_type*/
          , class _PointPrimitiveT     /*= typename _PointContainerT::value_type*/
          , class _FiniteFiniteDistFunctor
          >
 static inline int representBackCli(
         std::string                     cloud_path,
         std::string                     input_prims_path,
         std::string                     associations_path,
         std::string                     representativesPath,
         float                           scalar,
         AnglesT                         angle_gens)
 {
     const int verbose = 1;
     // input
     typedef typename _PrimitiveContainerT::value_type        InnerPrimitiveContainerT;
     typedef containers::PrimitiveContainer<_PrimitiveT>      PrimitiveMapT;
     typedef typename _PrimitiveT::Scalar                     Scalar;
     typedef typename _PrimitiveT::ExtremaT                   ExtremaT;
     PointContainerT         points;
     PclCloudPtrT            pcl_cloud;
     PrimitiveMapT           patches,reprPatches;
     RepresentParams<Scalar> params;
     _PrimitiveContainerT    prims;

     // Graphs
     typedef Graph<Scalar, typename MyGraphConfig<Scalar>::UndirectedGraph > GraphT;
     typedef typename graph::EdgeT<Scalar>                                   EdgeT;
     typedef typename GraphT::ComponentListT                                 ComponentListT;
     typedef typename GraphT::ClustersT                                      ClustersT;

     bool valid_input = true;
     if (!boost::filesystem::exists(cloud_path) )
     {
         std::cerr << "[" << __func__ << "]: " << "--cloud does not exist: " << cloud_path << std::endl;
         valid_input = false;
     }
     if (!input_prims_path.size() || (!boost::filesystem::exists(input_prims_path)) )
     {
         std::cerr << "[" << __func__ << "]: " << "-p or --prims is compulsory" << std::endl;
         valid_input = false;
     }
     if (!boost::filesystem::exists(associations_path))
     {
         std::cerr << "[" << __func__ << "]: " << "-a or --assoc is compulsory" << std::endl;
         valid_input = false;
     }
     params.scale = scalar;

     int ret = rapter::myParseInput<InnerPrimitiveContainerT,PclCloudT>(
                 points, pcl_cloud, prims, patches, cloud_path,input_prims_path,associations_path);



     std::cout << "[" << __func__ << "]: " << "parseInput ret: " << ret << std::endl;
     valid_input = (EXIT_SUCCESS == ret);

     params.angles = angle_gens;

     _PrimitiveContainerT representatives;
     valid_input &= (EXIT_SUCCESS == rapter::io::readPrimitives<_PrimitiveT, InnerPrimitiveContainerT>( representatives, representativesPath, &reprPatches) );
     if ( !valid_input ) std::cout << "failed read" << std::endl;


     if ( !valid_input )
     { std::cerr << "Usage: --representBack[3D] -p prims.csv -a points_primitives.csv -sc scale --cloud cloud.ply --repr representatives.bonmin.csv --angle-gens 90" << std::endl; return EXIT_FAILURE; }

     GidPidVectorMap populations;
     processing::getPopulations( populations, points );

     std::cout << "reprback" << std::endl;

     // loop over representative output to check for ID matches
     std::map<DidT, _PrimitiveT const*> subs;
     for ( typename PrimitiveMapT::Iterator it(reprPatches); it.hasNext(); it.step() )
     {
         if ( patches[ it.getGid() ].size() > 1 )
         {
             std::cerr << "[" << __func__ << "]: " << "can't handle more than one primitive per patch( gid: " << it.getGid() << "!" << std::endl;
             //return 1;
         }
         //for ( int )
         const int dId = patches[ it.getGid() ].at(0).getTag(_PrimitiveT::TAGS::DIR_GID);
         std::cout << "did at " << it.getGid() << " is " << dId << std::endl;

         if ( it.getDid() != dId )
         {
             if ( verbose ) std::cout << "found subst: " << it.getDid() << " instead of " << dId << " at gid " << it.getGid() << std::endl;
             if (    (subs.find( dId )                              != subs.end() )
                  && (subs[dId]->getTag(_PrimitiveT::TAGS::DIR_GID) != it.getDid()) )
                 std::cerr << "duplicate subs for patch " << it.getGid() << ": " << subs[dId]->getTag(_PrimitiveT::TAGS::DIR_GID) << ", " << it.getDid() << "!" << std::endl;
             else
                 subs[ dId ] = &(*it);
         }
     }

     _PrimitiveContainerT outPrims;
     for ( typename PrimitiveMapT::Iterator it(patches); it.hasNext(); it.step() )
     {
         bool copy = false;

         copy = (it->getTag(_PrimitiveT::TAGS::STATUS) == _PrimitiveT::STATUS_VALUES::SMALL );

         if ( !copy && (subs.find(it.getDid()) != subs.end()) )
         {
             _PrimitiveT const* subExample = subs[ it.getDid() ]; // pattern, to copy direction from
             std::cout << "substituting " << it.getGid() << "," << it.getDid() << " <- " << subExample->getTag(_PrimitiveT::TAGS::DIR_GID) << std::endl;

             int closest_angle_id = 0;
             _PrimitiveT sub;
             Scalar angdiff = MyPrimitivePrimitiveAngleFunctor::template eval<Scalar>( *it, *subExample
                                                                                     , params.angles
                                                                                     , &closest_angle_id );
             if ( angdiff > 0.1 )
                 std::cerr << "[" << __func__ << "]: " << "really? substitute dId, but best angdiff: " << angdiff << std::endl;

             if ( !it->generateFrom(/* out */ sub, /* example: */ *subExample, closest_angle_id, params.angles, /* sign, unused: */ Scalar(1.)) )
             {
                 std::cerr << "[" << __func__ << "]: " << "could not generate candidate" << std::endl;
                 copy = true;
             }
             else
             {
                 sub.setTag( _PrimitiveT::TAGS::STATUS, it->getTag(_PrimitiveT::TAGS::STATUS) );
                 containers::add( outPrims, it.getGid(), sub );
             }
         }
         else
             copy = true;

         if ( copy )
         {
             containers::add( outPrims, it.getGid(), *it );
         }
     }
     std::string prefix=cloud_path.substr(0,cloud_path.find_last_of("/")+1);

     return io::savePrimitives<_PrimitiveT, typename InnerPrimitiveContainerT::const_iterator >( outPrims, prefix + "subs.csv" );
 } //...representBack
}

